clear all;
close all;


j0=0.095;
b0=0.087;
kt0=0.8;
r=0.05;
a=0.25;
b=0.35;
dmax=2;

Qk=[-2,0,4,0,1.2,0];% ode45不能直接用矩阵，得变成向量进行操作；依次是x dx y dy phi dphi omega1 omega2 omega3 omega4
v=[0;0;0;0];
Zk=[0;0;0];



rho1=0.2;
rho2=0.5;
rho3=0.4;
rho4=0.4;
rho5=0.15;
rho6=0.4;
Ta=10;
Tb=10;
Tc=10;
Td=10;
Te=10;
Tf=10;

c1=2^(-1+rho1/2);
c2=3^(rho1/2)*2^(-1-rho1/2);
c3=2^(-1+rho2/2);
c4=3^(rho2/2)*2^(-1-rho2/2);

% c1=1;
% c2=1;
% c3=1;
% c4=1;



alpha=1.1;
beta=1.1;

kai1=20;
kai2=0.1;
lem=0.3;
eta=1;
delta_est_part1=0;
delta_est_part2=0;
delta_est_part3=0;

uwin=zeros(4,5);



T=0.001;
for k=1:1:80000
  
    time(k)=k*T;

%     xd(k)=3*sin(k*T);
%     dxd(k)=3*cos(k*T);
%     ddxd(k)=-3*sin(k*T);
%     yd(k)=3*sin(2*k*T);
%     dyd(k)=6*cos(2*k*T);
%     ddyd(k)=-12*sin(2*k*T);
%     phid(k)=3*cos(k*T);
%     dphid(k)=-3*sin(k*T);
%     ddphid(k)=-3*cos(k*T);

%     xd(k)=2*sin(0.1*k*T);
%     dxd(k)=0.2*cos(0.1*k*T);
%     ddxd(k)=-0.02*sin(0.1*k*T);
%     yd(k)=2*sin(0.2*k*T);
%     dyd(k)=0.4*cos(0.2*k*T);
%     ddyd(k)=-0.08*sin(0.2*k*T);
%     phid(k)=0;
%     dphid(k)=0;
%     ddphid(k)=0;

    xd(k)=2*sin(0.1*k*T);
    dxd(k)=0.2*cos(0.1*k*T);
    ddxd(k)=-0.02*sin(0.1*k*T);
    yd(k)=0;
    dyd(k)=0;
    ddyd(k)=0;
    phid(k)=0;
    dphid(k)=0;
    ddphid(k)=0;

    
    Qd(:,k)=[xd(k);yd(k);phid(k)];
    dQd(:,k)=[dxd(k);dyd(k);dphid(k)];
    ddQd(:,k)=[ddxd(k);ddyd(k);ddphid(k)];
    
             
            
    dt(:,k)=0*[sin(0.5*k*T);cos(0.1*k*T);sin(0.1*k*T);cos(0.7*k*T)];
%     dt(:,k)=[0;0;0;0];


    xq(k)=Qk(1);
    dxq(k)=Qk(2);
    yq(k)=Qk(3);
    dyq(k)=Qk(4);
    phiq(k)=Qk(5);
    dphiq(k)=Qk(6);


    Z(:,k)=Zk;
    
    Q(:,k)=[xq(k);yq(k);phiq(k)];
    dQ(:,k)=[dxq(k);dyq(k);dphiq(k)];


    
    
    e(:,k) = Q(:,k)-Qd(:,k);
    e1=e(1,k);e2=e(2,k);e3=e(3,k);
    de(:,k) = dQ(:,k)-dQd(:,k);
    de1=de(1,k);de2=de(2,k);de3=de(3,k); 
    
    
    phi=phiq(k)+pi/4;
    hinv=1/4*[-sqrt(2)*sin(phi),sqrt(2)*cos(phi),a+b;
               sqrt(2)*cos(phi),sqrt(2)*sin(phi),-(a+b);
              -sqrt(2)*sin(phi),sqrt(2)*cos(phi),-(a+b);
               sqrt(2)*cos(phi),sqrt(2)*sin(phi),(a+b)] ;  
           
    h=[-sqrt(2)*sin(phi),sqrt(2)*cos(phi),-sqrt(2)*sin(phi),sqrt(2)*cos(phi);
        sqrt(2)*cos(phi),sqrt(2)*sin(phi),sqrt(2)*cos(phi),sqrt(2)*sin(phi);
        1/(a+b)         ,-1/(a+b)        ,-1/(a+b)         ,1/(a+b)];  

    A=[-b0/j0    ,   -dphiq(k),   0;
          dphiq(k)    -b0/j0,        0;
          0,                0,         -b0/j0];

    g=kt0*r/(4*j0)*h;
    ginv=4*j0/(r*kt0)*hinv;
    
    delta(:,k)=-r/(4*j0)*h*dt(:,k);
%     delta_sat(:,k)=delta(:,k)+g*(vt_sat(:,k)-vt(:,k));

    s(:,k)=de(:,k)+pi/(rho1*Ta)*(c1*alpha*sigfunc(e(:,k),1-rho1)+c2/alpha*sigfunc(e(:,k),1+rho1));

    sob(:,k)=5*(Z(:,k)-dQ(:,k));
    so1=sob(1,k);so2=sob(2,k);so3=sob(3,k);
    delta_est_part1=delta_est_part1+lem/(1+lem-exp(-eta*abs(so1)))*sign(so1)*T;%
    delta_est(1,k)=-kai1*(sigfunc(so1,1-rho5)+sigfunc(so1,1+rho5))-kai2*delta_est_part1;
    delta_est_part2=delta_est_part2+lem/(1+lem-exp(-eta*abs(so2)))*sign(so2)*T;%
    delta_est(2,k)=-kai1*(sigfunc(so2,1-rho5)+sigfunc(so2,1+rho5))-kai2*delta_est_part2;
    delta_est_part3=delta_est_part3+lem/(1+lem-exp(-eta*abs(so3)))*sign(so3)*T;%
    delta_est(3,k)=-kai1*(sigfunc(so3,1-rho5)+sigfunc(so3,1+rho5))-kai2*delta_est_part3;

    M=(1-rho1)*[abs(e1)^-rho1,0,0;0,abs(e2)^-rho1,0;0,0,abs(e3)^-rho1];
    N=(1+rho1)*[abs(e1)^rho1,0,0;0,abs(e2)^rho1,0;0,0,abs(e3)^rho1];
    veq(:,k)=-ginv*[A*dQ(:,k)-ddQd(:,k)+pi/(rho1*Ta)*(c1*alpha*M*de(:,k))+c2/alpha*N*de(:,k)];
    vsw(:,k)=-ginv*[delta_est(:,k)+pi/(rho2*Tb)*(c3*beta*sigfunc(s(:,k),1-rho2)+c4/beta*sigfunc(s(:,k),1+rho2) )    + 0.1*tanh(100*s(:,k))];
    vt(:,k)=veq(:,k)+vsw(:,k);

   

    v=vt(:,k);
    vt_sat(:,k)=saturate(v,-120,120);
    v_sat=vt_sat(:,k);


     delta_sat(:,k)=delta(:,k)+g*(vt_sat(:,k)-vt(:,k));
        

    
    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plantmain(t,x,A,g,v_sat,delta(:,k)),tSpan,Qk);
    Qk = Qq(length(Qq),:);
   	[tt1,Zq] = ode45(@(t,x) plantdo(t,x,A,dQ(:,k),g,v,delta_est(:,k)),tSpan,Zk);
    Zk = Zq(length(Zq),:);
                              
                                   
     
end

v1=vt(1,:);
v2=vt(2,:);
v3=vt(3,:);
v4=vt(4,:);
v11=vt_sat(1,:);
v22=vt_sat(2,:);
v33=vt_sat(3,:);
v44=vt_sat(4,:);


e1=e(1,:);
e2=e(2,:);
e3=e(3,:);

de1=de(1,:);
de2=de(2,:);
de3=de(3,:);

S1=s(1,:);
S2=s(2,:);
S3=s(3,:);


figure(1);
subplot(311);
plot(time,xq,'k',time,xd,'r--');
xlabel('time(s)');ylabel('x');
legend('xq','xd');
grid on;
subplot(312);
plot(time,yq,'k',time,yd,'r--');
xlabel('time(s)');ylabel('y');
legend('yq','yd');
grid on;
subplot(313);
plot(time,phiq,'k',time,phid,'r--');
xlabel('time(s)');ylabel('phi');
legend('φq','φd');
grid on;

figure(2);
subplot(311);
plot(time,dxq,'k',time,dxd,'r--');
xlabel('time(s)');ylabel('dx');
legend('dxq','dxd');
grid on;
subplot(312);
plot(time,dyq,'k',time,dyd,'r--');
xlabel('time(s)');ylabel('dy');
legend('dyq','dyd');
grid on;
subplot(313);
plot(time,dphiq,'k',time,dphid,'r--');
xlabel('time(s)');ylabel('dphi');
legend('dφq','dφd');
grid on;
 
 
 
 

figure(3);
subplot(311);
plot(time,e1,'k',time,de1,'r');
xlabel('time(s)');ylabel('e1');
grid on;
subplot(312);
plot(time,e2,'k',time,de2,'r');
xlabel('time(s)');ylabel('e2');
grid on;
subplot(313);
plot(time,e3,'k',time,de3,'r');
xlabel('time(s)');ylabel('e3');
grid on;


figure(4);
subplot(311);
plot(time,S1,'k');
xlabel('time(s)');ylabel('s1');
grid on;
subplot(312);
plot(time,S2,'k');
xlabel('time(s)');ylabel('s2');
grid on;
subplot(313);
plot(time,S3,'k');
xlabel('time(s)');ylabel('s3');
grid on;


figure(5);
plot(xq,yq,'k',xd,yd,'r--','linewidth',1);
xlabel('x');ylabel('q');
legend('actual trace','ideal trace');

figure(6);
subplot(221);
plot(time,v1,'k');
xlabel('time(s)');ylabel('u1');
grid on;
subplot(222);
plot(time,v2,'k');
xlabel('time(s)');ylabel('u2');
grid on;
subplot(223);
plot(time,v3,'k');
xlabel('time(s)');ylabel('u3');
grid on;
subplot(224);
plot(time,v4,'k');
xlabel('time(s)');ylabel('u4');
grid on;

figure(66);
subplot(221);
plot(time,v11,'k');
xlabel('time(s)');ylabel('u1');
grid on;
subplot(222);
plot(time,v22,'k');
xlabel('time(s)');ylabel('u2');
grid on;
subplot(223);
plot(time,v33,'k');
xlabel('time(s)');ylabel('u3');
grid on;
subplot(224);
plot(time,v44,'k');
xlabel('time(s)');ylabel('u4');
grid on;

figure(7);
subplot(311);
plot(time,delta_sat(1,:),'k',time,delta_est(1,:),'r--');
xlabel('time(s)');ylabel('s1');
grid on;
subplot(312);
plot(time,delta_sat(2,:),'k',time,delta_est(2,:),'r--');
xlabel('time(s)');ylabel('s2');
grid on;
subplot(313);
plot(time,delta_sat(3,:),'k',time,delta_est(3,:),'r--');
xlabel('time(s)');ylabel('s3');
grid on;

figure(77);
subplot(311);
plot(time,delta_sat(1,:)-delta_est(1,:),'k',time,sob(1,:),'b--');
xlabel('time(s)');ylabel('s1');
grid on;
subplot(312);
plot(time,delta_sat(2,:)-delta_est(2,:),'k',time,sob(1,:),'b--');
xlabel('time(s)');ylabel('s2');
grid on;
subplot(313);
plot(time,delta_sat(3,:)-delta_est(3,:),'k',time,sob(1,:),'b--');
xlabel('time(s)');ylabel('s3');
grid on;
figure(8);

plot(time,vt_sat,'k');%,time,de1,'r',time,de2,'r',time,de3,'r'
xlabel('time(s)');ylabel('e1');

% figure(7);
% subplot(221);
% plot(time,ux(1,:),'k');
% xlabel('time(s)');ylabel('ux1');
% grid on;
% subplot(222);
% plot(time,uxeq(1,:),'k');
% xlabel('time(s)');ylabel('ux1eq');
% grid on;
% subplot(223);
% plot(time,uxsw(1,:),'k');
% xlabel('time(s)');ylabel('ux1sw');
% grid on;
% subplot(224);
% plot(time,uxsw1(1,:),'k');
% xlabel('time(s)');ylabel('ux1sw1');
% grid on;



